/* GPS interface */

/* Copyright (C) 2009 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */


#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "global.h"
#include <util/delay.h>

#include "clock.h"
#include "serial.h"
#include "gps.h"

/* GPS parser states */
#define GPS_STATE_INIT 0
#define GPS_STATE_START 1
#define GPS_STATE_PAYLOAD 2
#define GPS_STATE_COMPLETE 3

#define GPS_PKTLEN 83

//ISR(PCINT1_vect) {
// if (PINB & (1 << PINB0)) {
//  if (!(flags & FLAG_GPS_PPS)) {
//   flags |= FLAG_GPS_PPS;
//   clock_us_pps = TCNT1;
//   clock_ms_pps = clock_frac;
//   clock_s_gps = clock_s;
//  }
// }
//}

// ISR(INT2_vect) {
// clock_us_pps = TCNT1;
// clock_ms_pps = clock_frac;
// clock_s_pps = clock_s;
// clock_s_gps = 0;
// clock_ms_gps = 0;
// flags |= FLAG_GPS_PPS; /* we are allowed to do this as we're in an interrupt */
// }

void gps_requestversion(void) {
 serial1_tx_cout(0x10);
 serial1_tx_cout(0x1c);
 serial1_tx_cout(0x03);
 serial1_tx_cout(0x10);
 serial1_tx_cout(0x03);
 return;
}

void gps_init(void) {

 /* make sure PPS flag is not set */
 clear_flag(FLAG_GPS_PPS);

// serial0_tx_PGM(PSTR("gps: pps interrupt enabled\r\n"));

 serial1_init(38400);

// DDRB |= (1 << PORTB1); /* GPS Status LED */

 _delay_ms(10);

 /* silence packets we don't care about seeing automatically */
 serial1_tx("\x10\x35\x00\x00\x10\x10\x00\x10\x03",9);

// serial1_tx_cout(0x10); /* command start */
// serial1_tx_cout(0x35); /* Set/Request IO Options, pg 127 */
// serial1_tx_cout(0x00); /* Disable position reporting */
// serial1_tx_cout(0x00); /* Disable velocity reporting */
// serial1_tx_cout(0x10); /* PPS Mode fixed based, GPS time */
// serial1_tx_cout(0x10); /* exscaping of end sequence */
// serial1_tx_cout(0x00); /* no raw measurements, signal level AMU, signal off */
// serial1_tx_cout(0x10);
// serial1_tx_cout(0x03); /* end packet */

}

void gps_process(void) {
 static uint8_t pkt[GPS_PKTLEN+1];
 static uint8_t n = 0, state = GPS_STATE_INIT, dle_seen = 0;
 static uint8_t s;

 if (!(flags & FLAG_USART1_RX)) {
  return;
 }

 switch (state) {
	case GPS_STATE_INIT:
 		/* skip packets until we see the correct end sequence */
  		s = serial1_rx();
		if (s == 0x10) {
		 dle_seen = 1 - dle_seen;
		 /* we're done for now, since we're in init */
		 break;
		}
		if (s == 0x03 && dle_seen) {
//		 serial0_tx_PGM(PSTR("gps: tsip parser synced\r\n"));
		 dle_seen = 0;
		 state = GPS_STATE_START;
		 n = 0;
		 /* request the version so we're in alignment */
		 break;
		}
  		dle_seen = 0;
		break;
	case GPS_STATE_START:
		/* packet is a start packet, right? */
		s = serial1_rx();
		if (s != 0x10) {
		 serial0_tx_PGM(PSTR("gps: out of sync, resyncing\r\n"));
		 /* we're out of sync, give up */
		 state = GPS_STATE_INIT;
		 break;
		}
		state = GPS_STATE_PAYLOAD;
		break;
	case GPS_STATE_PAYLOAD:
		/* copy the payload of the packet */
		s = serial1_rx();
		if (s == 0x10) {
		 if (!dle_seen) {
		  pkt[n++] = s;
 		 }
		 dle_seen = 1 - dle_seen;
		 break;
		}
		if (s == 0x03 && dle_seen) {
		 n--; /* we had a DLE just before, not packet of packet */
		 state = GPS_STATE_COMPLETE;
		 dle_seen = 0;
		 break;
		}
		pkt[n++] = s; /* copy the packet */ 
		dle_seen = 0;
		break;
	default:
		serial0_tx_PGM(PSTR("gps: BUG: parse error\r\n"));
		break;
 }

 /* process the completed packet if we have one */

 if (state == GPS_STATE_COMPLETE) {
  /* handle this packet */
  switch (pkt[0]) {
	case 0x41:
		/* time packet */
		/* pg 130 */
		if (!(flag_gps & FLAG_GPS_LOCK)) {
		 break; /* we ignore these packets without a full lock */
		}

		/* if we don't have a PPS flag, then we ignore this
		 * as a rogue packet */
		if (!(flags & FLAG_GPS_PPS)) {
		 serial0_tx_PGM(PSTR("gps: got 0x46; no PPS signal?\r\n"));
		 break;
		}

		/* clear flag, we've got the first and only packet for
		 * this second, right? */
		clear_flag(FLAG_GPS_PPS);

		/* decode the time into clock*gps variables */
		
		/* first, base in seconds. This is just NTP->GPS base
		   plus the week number multiplied by seconds in the
		   week */
		clock_s_gps = 315964819 + ((pkt[5] << 8)+pkt[6]) * 604800;

		/* now apply the time of week to it */
		{ 
		 float f;

		 /* hack: byte copy into a float */
		 /* AVR are LSB first, wire is MSB first */
		 *((uint8_t *) &f) = pkt[4];
		 *((uint8_t *) &f + 1) = pkt[3];
		 *((uint8_t *) &f + 2) = pkt[2];
		 *((uint8_t *) &f + 3) = pkt[1];

		 clock_s_gps += (uint32_t) f;
		
		 /* and now multiple out the 1000ths of a second
		    to store too */
//		 clock_ms_gps = ((uint32_t) (f * 1000.0)) % 1000;
		 clock_ms_gps = 0; /* it will always be 0 */
		}

		/* and now apply the UTC leap seconds correction */
		{
		 float f;

                 /* hack: byte copy into a float */
                 /* AVR are LSB first, wire is MSB first */
                 *((uint8_t *) &f) = pkt[10];
                 *((uint8_t *) &f + 1) = pkt[9];
                 *((uint8_t *) &f + 2) = pkt[8];
                 *((uint8_t *) &f + 3) = pkt[7];
		
		 /* UTC offset is always integer second even tho
		  * we get a float, go figure */
		 clock_s_gps -= (uint32_t) f;
  		 clock_s_gps -= 19; /* convert GPS to TAI */
		}

		/* indicate we have the current GPS time */
		set_flag(flag_gps,FLAG_GPS_EVENT_TIME);

		global_counters.gps_frame_good++;

		break;
	case 0x46:
		if (n != 3) {
		 /* getting strange 0x46 packets that are short? */
		 global_counters.gps_frame_bad++;
		 break;
		}
		/* receiver health status */
		if (pkt[1] == 0 && !(flag_gps & FLAG_GPS_LOCK)) {
		 set_flag(flag_gps, FLAG_GPS_LOCK | FLAG_GPS_EVENT_LOCK);
		 serial0_tx_PGM(PSTR("gps: locked\r\n"));
		 //PORTB |= (1 << PORTB1);
		}
		if (pkt[1] != 0 && (flag_gps & FLAG_GPS_LOCK)) {
		 clear_flag(flag_gps, FLAG_GPS_LOCK | FLAG_GPS_EVENT_LOCK);
		 serial0_tx_PGM(PSTR("gps: lost lock "));
		 serial0_tx_hex(pkt[1]);
		 serial0_tx_cout('\r');
		 serial0_tx_cout('\n');
		 //PORTB &= ~(1 << PORTB1);
		}
                global_counters.gps_frame_good++;
		break; 
//	case 0x55:
		/* IO Options Report */
		/* pg 136 */
		/* Strictly speaking, we should compare this to what
		   we feed in the init phase */
		serial0_tx_PGM(PSTR("gps: settings accepted\r\n"));
		break;
	case 0x6D:
		/* All-in-view Satellite Selection */
		/* Pg 144 */
		
		/* update satellite count */
	        gps_sat_count = (pkt[1] & 0x7);

//		if (!(flags & FLAG_GPS_EVENT_LOCK)) {
//		 break;
//		}
//		clear_flag(FLAG_GPS_EVENT_LOCK);

//		serial0_tx_PGM(PSTR("gps: "));
//		if (pkt[1] & 0x7) {
//		 serial0_tx_dec((pkt[1] & 0x7) - 1);
//		 serial0_tx_PGM(PSTR("D fix ("));
//		 serial0_tx_dec(pkt[1] >> 4);
//		 serial0_tx_PGM(PSTR(" satellites)\r\n"));
//		} else {
//		 serial0_tx_PGM(PSTR("no fix\r\n"));
//		}
                global_counters.gps_frame_good++;
		break;
	case 0x1c:
		/* Hardware Version report */
		/* pg 122 */
		serial0_tx_PGM(PSTR("gps: "));
		{
		 uint8_t x;
		 for (x = 0; x < pkt[13]; x++) {
		  serial0_tx_cout(pkt[14+x]);
		 }
		}
		serial0_tx_PGM(PSTR(" SN:"));
		serial0_tx_hex(pkt[2]);
		serial0_tx_hex(pkt[3]);
		serial0_tx_hex(pkt[4]);
		serial0_tx_hex(pkt[5]);
		serial0_tx_cout(' ');
		serial0_tx_dec((pkt[8] << 8) + pkt[9]);
		serial0_tx_cout('/');
		serial0_tx_dec(pkt[7]);
		serial0_tx_cout('/');
		serial0_tx_dec(pkt[6]);
		serial0_tx_cout('\r');
		serial0_tx_cout('\n');

		global_counters.gps_frame_good++;
		break;
	default:
		global_counters.gps_frame_ignored++;
  }
  /* reset state */
  state = GPS_STATE_START;
  n = 0;
 }

 return;

}

/* process NMEA data streams, currently unused */
void gps_process_nmea(void) {
 uint8_t s, n = 0, sum = 0, sumrx = 0;
 char pkt[GPS_PKTLEN+1];

// if (flags & FLAG_GPS_PPS) {
 // serial0_send_PGM(PSTR("gps: PULSE\r\n"));
 // flags &= ~(FLAG_GPS_PPS);
// }

 if (!(flags & FLAG_GPS_NMEA)) {
  return; /* nothing to do */
 }

 clear_flag(FLAG_GPS_NMEA);

 /* process the message we've just been given */
 s = serial1_rx();

 if (s != '$') {
  /* dump the rest of this message, it's alignment rubbish */
  serial0_tx_PGM(PSTR("gps: bogus packet\r\n"));
  while (flags & FLAG_USART1_RX && s != '\n') {
   s = serial1_rx();
  }
  return;
 }

 /* we have a packet to process, begin calculating the checksum */
 
 while (flags & FLAG_USART1_RX && s != '\n') {

  s = serial1_rx();

  if (s == '\n' || s == '*') {
   break;
  }

  if (!sum) {
   sum = s;
  } else {
   sum ^= s;
  }
  if (n < GPS_PKTLEN) {
   pkt[n++] = s;
  }
 }

 /* if we got to '*' then this is a valid packet */
 if (s == '*') {
  /* compare checksums */
  s = serial1_rx();
  if (s >= '0' && s <= '9') {
   sumrx = ((s - 0x30) << 4);
  }
  if (s >= 'A' && s <= 'F') {
   sumrx = ((s - 'A')+0xa) << 4;
  }
  s = serial1_rx();
  if (s >= '0' && s <= '9') {
   sumrx += (s - '0');
  } 
  if (s >= 'A' && s <= 'F') {
   sumrx += ((s - 'A') + 0xa);
  }
 
  if (sumrx == sum) {
   /* we have a valid checksum */

   /* display our checksum */
   serial0_tx_PGM(PSTR("gps: -> "));
   for (n = 0; n < GPS_PKTLEN; n++) {
    if (pkt[n] == ',') { 
     break;
    }
    serial0_tx_cout(pkt[n]);
   }
   serial0_tx_cout('\r');
   serial0_tx_cout('\n');
  } else {
   serial0_tx_PGM(PSTR("gps: invalid checksum on rx\r\n"));
  } 
 } else {
  serial0_tx_PGM(PSTR("gps: strange packet\r\n"));
 }

 /* absorb all other chars */
 while (flags & FLAG_USART1_RX && s != '\n') {
  s = serial1_rx();
 }

 return;

}

void gps_send_nmea(const char *str) {
 uint8_t sum = 0;
 /* send the command given, calculating checksum */
 serial1_tx_cout('$');
 while (*str) {
  serial1_tx_cout(*str);
  if (!sum) {
   sum = *str;
  } else {
   sum ^= *str;
  }
  str++;
 }
 serial1_tx_cout('*');
 serial1_tx_hex(sum);
 serial1_tx_cout('\r');
 serial1_tx_cout('\n');
}
 
